package robot.calibeta;

import chair.core.Diary;
import lejos.nxt.Motor;
import lejos.robotics.TachoMotor;

/**
 * Cette classe gère les déplacements du robot : tourner, avancer, distance
 * parcourue, etc. Permet aussi aux capteurs de savoir s'il est utile
 * d'effectuer des mesures.
 * 
 * Non on n'utilise pas le très pratique TachoPilot car entraîne des problèmes
 * de communication avec utilisation de puis PC à cause de la très grande
 * fréquence d'interrogation des capteurs. Motion se déclare occupé quand il
 * fait tourner le robot ou qu'il le déplace sur une certaine distance afin de
 * permettre aux organes de se mettre en attente.
 * 
 * @author nomhad
 * 
 */
public class Motion {

	/** Emprunté à TachoPilot */
	private final static float _trackWidth = 150;
	private final static float _wheelDiameter = 42;
	private final static float _turnRatio = _trackWidth / _wheelDiameter;
	private final static TachoMotor _left = Motor.C;
	private final static TachoMotor _right = Motor.B;
	private final static float _degPerDistance = 360 / ((float) Math.PI * _wheelDiameter);

	/** Où se trouve l'avant du robot (son cap) par rapport au "Nord" du début */
	private static float _capeAngle = 0;
	/** Quel est l'angle que forme le robot par rapport à son cap */
	private static float _currentAngle = 0;

	/** Est-ce que le robot est indisposé pour cause de déplacement fin ? */
	private static boolean _isWorking = false;

	/** Robot en attente ? */
	private static boolean _isOnABreak = false;

	/**
	 * De combien de degrés par rapport au nord le robot doit-il se tourner par
	 * rapport au Nord si doit zigzaguer de manière optimale (utilisé par
	 * BehaviorGoStraight)
	 */
	public final static float TURN_ANGLE = 30;

	/**
	 * Le robot tourne au plus court et se déplacera en marche arrière si
	 * besoin.
	 */
	private static boolean _rotateShortest = false;
	/**
	 * Dans le cas où robot fait marche arrière quand rotation de 180°, modifie
	 * comportement de forward() et de travel()
	 */
	private static boolean _isMovingForward = true;

	/**
	 * Règle les paramètres de base comme la vitesse du robot
	 */
	public static void init() {
		// Je fais tout comme TachoPilot
		Motor.B.setSpeed(360);
		Motor.C.setSpeed(360);
	}

	/**
	 * Amène le robot à un certain angle par rapport à son cap. Ainsi rotate(0)
	 * au lieu de ne rien faire aligne le robot avec son référentiel actuel. Le
	 * robot sera arrêté après la rotation.
	 * 
	 * NB: ne pas oublier que si on est pas en mode RotWhenOpposite demander 180
	 * degrés ne fera rien. Il faut donc ruser avec 179 par exemple.
	 * 
	 * TODO: un paramètre en plus pour forcer demi-tour, ou bien une autre
	 * fonction.
	 * 
	 * @param angle
	 *            de combien de degrés doit-on faire tourner le robot par
	 *            rapport à son cap actuel
	 */
	synchronized public static void rotateFromCape(float angle) {
		// De combien de degrés doit on faire effectivement tourner le robot
		float compAngle = Cartographer.normalizeAngle(angle
				+ getCapeDeviation());
		_currentAngle += compAngle;
		// On normalise
		_currentAngle = Cartographer.normalizeAngle(_currentAngle);
		rotate(compAngle);
	}

	/**
	 * Effectue réellement la rotation. Dans le mode rotateShortest ce ne sera
	 * pas forcément l'avant du robot qui adoptera l'angle, mais l'arrière si
	 * c'est plus court.
	 * 
	 * NB: l'angle aura été normalisé avant appel, compris dans ]-180,180]
	 * 
	 * @param angle
	 *            de combien de degrés le robot doit tourner
	 */
	synchronized private static void rotate(float angle) {
		Diary.logln("angle début : " + angle);

		// Si on a décidé de tourner au plus court, où que se trouve l'avant du
		// réel du robot (lombric style)
		if (_rotateShortest) {
			// L'angle est plus proche de l'autre côté (== 180 - angle < angle)
			if (Math.abs(angle) > 90) {
				_isMovingForward = !_isMovingForward;
				angle = Cartographer.normalizeAngle(angle - 180);
			}
			// facile avec 180° : signale qu'il faut inverser les futur commande
			// et retourne
			/*
			 * if (angle == 180) { _isMovingForward = !_isMovingForward; return;
			 * }
			 */
			// Sinon faut modifier angle dans le cas où est en marche arrière
			/*
			 * else if (!_isMovingForward) { angle =
			 * Cartographer.normalizeAngle(angle - 180); }
			 */
		}

		int rotateAngle = (int) (angle * _turnRatio);
		// Si au final il n'y a aucune rotation on retourne
		if (rotateAngle == 0)
			return;

		// Arrête capteur pendant cette phase
		_isWorking = true;

		_left.rotate(-rotateAngle, true);
		_right.rotate(rotateAngle);

		_isWorking = false;
		Diary.logln("angle fin : " + angle);
	}

	/**
	 * Remplace le cap actuel par un nouveau. Ainsi pour indiquer l'Ouest :
	 * changeCape(-90).
	 * 
	 * Attention, aucune rotation ne sera inculquée au robot, il faudra appeler
	 * rotateFromCape(0) pour que le robot se dirige effectivement dans cette
	 * direction lors de l'appel de travel().
	 * 
	 * @param cape
	 *            angle par rapport au nord.
	 */
	public static void changeCapeAngle(float cape) {
		_capeAngle = cape;
		_capeAngle = Cartographer.normalizeAngle(_capeAngle);
	}

	/**
	 * Corrige le cap actuel. Ainsi pour faire tourner le cap vers la droite (et
	 * non le robot !) : alterCape(-90).
	 * 
	 * NB: à utiliser avec parcimonie, par exemple si le robot commence à trop
	 * se tromper dans ses angles.
	 * 
	 * @param angle
	 *            angle par rapport au cap
	 */

	public static void alterCape(float angle) {
		_capeAngle += angle;
		_capeAngle = Cartographer.normalizeAngle(_capeAngle);
	}

	/** Arrête le robot */
	public static void stop() {
		_left.stop();
		_right.stop();
	}

	/**
	 * Retourne la distance positive parcourue depuis le lancement du robot. Si
	 * robot recule, résultat décrémente.
	 * 
	 * @return distance en mm
	 */
	public static float getTravelDistance() {
		float left = (_left.getTachoCount()) / _degPerDistance;
		float right = _right.getTachoCount() / _degPerDistance;
		float total = (left + right) / 2.0f;
		return total;
	}

	/**
	 * Fait avancer le robot dans la direction de son cap (le fait reculer le
	 * cas échéant suivant gestion de _rotateWhenOpposite)
	 */
	public static void forward() {
		if (_isMovingForward) {
			_left.forward();
			_right.forward();
		} else {
			_left.backward();
			_right.backward();
		}
	}

	/**
	 * Calcule la différence entre le cap du robot et sa rotation actuelle
	 * 
	 * @return à combien de degrés se trouve le cap
	 */
	public static float getCapeDeviation() {
		return Cartographer.normalizeAngle((_capeAngle - _currentAngle));
	}

	/**
	 * Calcule la différence entre le nord du robot et sa rotation actuelle
	 * 
	 * @return à combien de degrés se trouve le nord
	 */
	public static float getNorthDeviation() {
		return Cartographer.normalizeAngle(-_currentAngle);
	}

	/**
	 * Parcours une certaine distance en ligne droite.
	 * 
	 * @param distance
	 *            distance en mm
	 */
	public static void travel(float distance) {
		// Dans les faits le robots fait une marche arrière
		if (!_isMovingForward) {
			distance *= -1;
		}
		_isWorking = true;
		_left.rotate((int) (distance * _degPerDistance), true);
		_right.rotate((int) (distance * _degPerDistance));
		_isWorking = false;
	}

	/**
	 * Renseigne les Organ sur l'indisposition éventuelle du robot.
	 * 
	 * @return true si rotate() ou travel() en cours
	 */
	public static boolean isWorking() {
		return _isWorking;
	}

	/**
	 * Utilisé par OrganEyeColor car ce dernier en cas de déplacement est plus
	 * prudent lorsqu'il renseigne des couleurs.
	 * 
	 * @return true si le robot s'accorde une petite pause immobile
	 */
	public static boolean isOnABreak() {
		return _isOnABreak;
	}

	/**
	 * Appel bloquant : arrête le robot pendant un certain temps. Utilisé par
	 * Cartographer lors de l'exploration d'une branche. On n'utilise pas les
	 * méthodes isMoving() de chaque moteur car le délais après des rotations
	 * est un faux positif.
	 * 
	 * @param time
	 *            le temps pendant lequel le Thread courant va être endormi
	 */
	synchronized public static void haveABreak(long time) {
		stop();
		_isOnABreak = true;
		try {
			Thread.sleep(time);
		} catch (InterruptedException e) {
		}
		_isOnABreak = false;
	}

	/**
	 * Une version non bloquante, histoire que ReflexFreeze ne monopolise pas
	 * Striatum alors qu'il existe déjà un mécanisme de timer.
	 * 
	 * Attention, on pourra toujours demander à faire bouger le robot si on
	 * appelle une autre méthode.
	 * 
	 * TODO: enfiler les ordres quand en pause
	 * 
	 * @param toggle
	 *            le robot est-il marqué en pause ?
	 */
	synchronized public static void haveABreak(boolean toggle) {
		_isOnABreak = toggle;
		if (toggle)
			stop();
	}

	/**
	 * 
	 * Par défaut, quand on demande au robot de tourner c'est l'avant qui
	 * pointera dans la nouvelle direction. Mais si le sens nous importe peu
	 * (indifférent à ce que le robot se déplace en avant ou en arrière) il est
	 * parfois plus court de tourner l'arrière dans la direction choisi. C'est
	 * ici de ce mode qu'il est question.
	 * 
	 * Si on désactive ce comportement, va remettre tout de suite l'avant du
	 * robot dans le bon axe.
	 * 
	 * @param rotateShortest
	 *            true pour que le robot adopte la direction au plus vite
	 *            quelque soit son sens, false (défaut) pour que ce soit
	 *            toujours l'avant du robot qui se retrouve au bon angle
	 */
	public static void setRotateShortest(boolean rotateShortest) {
		_rotateShortest = rotateShortest;
		// On était en marche arrière et on veut revenir à un comportement
		// normal : remet le robot dans le bon sens
		// FIXME: à tester
		if (!rotateShortest && !_isMovingForward) {
			rotate(180);
			_isMovingForward = true;
		}
	}
}
